using System;
using System.Collections.Generic;
using System.Text;
using Geometry;
using Mathematics.Matrice;

namespace Photogrammetry.Transformation
{
    /// <summary>
    /// This class calculates the affine transformation parameters and transformed coordinates
    /// of the given coordinates.
    /// </summary>
    class Affine
    {
        public Matrice a = new Matrice(6, 1);

        /// <summary>
        /// This method calculates the transformation parameters with the first 3 vertices in parameters.
        /// It is just for optimisation if no estimation is needed.
        /// </summary>
        /// <param name="pictureVertices">
        /// Coordinates of the picture by using an array of Point structure.
        /// </param>
        /// <param name="realVertices">
        /// Real coordinates by using an array of Point structure.
        /// </param>
        /// <returns>void</returns>
        private void AffineUnControlled(Point[] pictureVertices, Point[] realVertices)
        {
            double[] p = new double[5];

            p[0] = (pictureVertices[2].Y - pictureVertices[0].Y) * (realVertices[1].X - realVertices[0].X) - (pictureVertices[1].Y - pictureVertices[0].Y) * (realVertices[2].X - realVertices[0].X);
            p[1] = (pictureVertices[1].X - pictureVertices[0].X) * (realVertices[2].X - realVertices[0].X) - (pictureVertices[2].X - pictureVertices[0].X) * (realVertices[1].X - realVertices[0].X);
            p[2] = (pictureVertices[2].Y - pictureVertices[0].Y) * (realVertices[1].Y - realVertices[0].Y) - (pictureVertices[1].Y - pictureVertices[0].Y) * (realVertices[2].Y - realVertices[0].Y);
            p[3] = (pictureVertices[1].X - pictureVertices[0].X) * (realVertices[2].Y - realVertices[0].Y) - (pictureVertices[2].X - pictureVertices[0].X) * (realVertices[1].Y - realVertices[0].Y);
            p[4] = (pictureVertices[1].X - pictureVertices[0].X) * (pictureVertices[2].Y - pictureVertices[0].Y) - (pictureVertices[2].X - pictureVertices[0].X) * (pictureVertices[1].Y - pictureVertices[0].Y);

            a[0, 0] = p[0] / p[4];
            a[1, 0] = p[1] / p[4];
            a[3, 0] = p[2] / p[4];
            a[4, 0] = p[3] / p[4];

            a[2, 0] = realVertices[0].X - a[0, 0] * pictureVertices[0].X - a[1, 0] * pictureVertices[0].Y;
            a[5, 0] = realVertices[0].Y - a[3, 0] * pictureVertices[0].X - a[4, 0] * pictureVertices[0].Y;
            Console.WriteLine(a.ToString());
        }

        /// <summary>
        /// This method constructs the affine object. If given more then 3 parameters
        /// then it uses the least squares estimation for accuracy else it uses the 
        /// AffineUnControlled method.
        /// </summary>
        /// <param name="pictureVertices">
        /// Coordinates on the picture by using an array of Point structure.
        /// </param>
        /// <param name="realVertices">
        /// Real coordinates by using an array of Point structure.
        /// </param>
        /// <returns>Object(class : Photogrammetry.Transformation.Affine)</returns>
        public Affine(Point[] pictureVertices, Point[] realVertices)
        {
            if (pictureVertices.GetLength(0) > 4)
            {
                Matrice Kc = new Matrice(pictureVertices.GetLength(0) * 2, 6);
                Matrice xReal = new Matrice(pictureVertices.GetLength(0) * 2, 1);
                for (int i = 0; i < pictureVertices.GetLength(0); i++)
                {
                    Kc[i, 0] = pictureVertices[i].X;
                    Kc[i, 1] = pictureVertices[i].Y;
                    Kc[i, 2] = 1;
                    Kc[i + pictureVertices.GetLength(0), 3] = pictureVertices[i].X;
                    Kc[i + pictureVertices.GetLength(0), 4] = pictureVertices[i].Y;
                    Kc[i + pictureVertices.GetLength(0), 5] = 1;
                    xReal[i, 0] = realVertices[i].X;
                    xReal[i + pictureVertices.GetLength(0), 0] = realVertices[i].Y;
                }
                a = Kc.TransposeNoChange() * Kc;
                a.Inverse();
                a = a * Kc.TransposeNoChange();
                a = a * xReal;
                Console.Write(a.ToString());
            }
            else
            {
                AffineUnControlled(pictureVertices, realVertices);
            }
        }

        /// <summary>
        /// This method transforms the coordinates by using parameters calculated in the Affine method.
        /// </summary>
        /// <param name="pictureVertices">Non-transformed coordinates to be transformed.</param>
        /// <returns>Transformed coordinates in a Point[].</returns>
        public Point[] Transform(Point[] pictureVertices)
        {
            Point[] realVertices = new Point[pictureVertices.GetLength(0)];
            for (int i = 0; i < pictureVertices.GetLength(0); i++)
            {
                realVertices[i] = new Point();
                realVertices[i].X = (pictureVertices[i].X * a[0, 0] + pictureVertices[i].Y * a[1, 0] + a[2, 0]);
                realVertices[i].Y = (pictureVertices[i].X * a[3, 0] + pictureVertices[i].Y * a[4, 0] + a[5, 0]);
            }
            return realVertices;
        }
    }
}
